clc;
clear;
close all;

addpath('autogen\')


% 1、验证DH参数 passed
%{
a = [0, 0, 0.0, 0];
alpha = [1.309, -pi/2, -pi/2, -pi/2];
d = [0, 0.0145, 0.3885, 0];
q = [-pi/2, -1.309, pi, 0];

for i=1:4
    T(:,:,i)=[
            cos(q(i)), -sin(q(i)), 0, a(i);
            sin(q(i)).*cos(alpha(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i)), -d(i)*sin(alpha(i));
            sin(q(i))*sin(alpha(i)), cos(q(i))*sin(alpha(i)), cos(alpha(i)), d(i)*cos(alpha(i));
            0,0,0,1];
end

% 在axis_0坐标系下某一点左边
P = [-0.101; 0.0; 0.2185; 1];
T01=T(:,:,1);
T02=T01*T(:,:,2);
T03=T02*T(:,:,3);
T04=T03*T(:,:,4);
% 转换到axis_4坐标系下，检测坐标变换是否正确？
newP = inv(T04)*P;
%}

% 2、生成雅可比函数 （与左腿相同）


% CAD评估
m1=0.42;m2=0.35;m3=0.66;m4=0.58;
r1_x=0.01;r1_y=-0.01;r1_z=0.01;
r2_x=0.00;r2_y=-0.09;r2_z=0.03;
r3_x=0.00;r3_y=-0.00;r3_z=0.11;
r4_x=-0.01;r4_y=-0.21;r4_z=0.02;

m1_r1 = m1*[r1_x;r1_y;r1_z;1];
m2_r2 = m2*[r2_x;r2_y;r2_z;1];
m3_r3 = m3*[r3_x;r3_y;r3_z;1];
m4_r4 = m4*[r4_x;r4_y;r4_z;1];

lb = [0, 0, 0, 0,...
    r1_x-0.01, r1_y-0.1, r1_z-0.01,...
    r2_x-0.0, r2_y-0.2, r2_z-0.03,...
    r3_x-0.0, r3_y-0.0, r3_z-0.11,...
    r4_x-0.1, r4_y-0.5, r4_z-0.02];

ub = [m1+1.0, m2+1.0, m3+1.0, m4+1.0,...
    r1_x+0.1, r1_y+0.01, r1_z+0.1,...
    r2_x+0.0, r2_y+0.09, r2_z+0.1,...
    r3_x+0.0, r3_y+0.0, r3_z+0.5,...
    r4_x+0.01, r4_y+0.21, r4_z+0.1];

alpha = [1.309, -pi/2, -pi/2, -pi/2]';

data = load('data_l3/r_leg.txt');
data = data(:, :);

qs = data(:, 1:4);
tor = data(:, 9:12);

g = [0 0 -9.8 0]';

K = [];
Tau = [];
tau_1s_real = [];tau_2s_real = [];tau_3s_real = [];tau_4s_real = [];
tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];
for i=1:1:length(qs)
    q = qs(i, :)'; 
    
    u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
    u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
    u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
    u44 = J_ll_T04_q4(q, alpha);
    
    %     
    tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4;
    tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4;
    tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
    tau_4 = -g'*u44*m4_r4;
    
    % [-3.07000000000000;-1.74000000000000;-0.497998000000000;1.50000000000000]
    % [-5.21, 0.9357, 2.4768, 0.0097]    
    
    tau_4s_real = [tau_4s_real; tor(i, 4)];
    tau_3s_real = [tau_3s_real; tor(i, 3)];
    tau_2s_real = [tau_2s_real; tor(i, 2)];
    tau_1s_real = [tau_1s_real; tor(i, 1)];
    
    tau_4s_est = [tau_4s_est; tau_4];
    tau_3s_est = [tau_3s_est; tau_3];
    tau_2s_est = [tau_2s_est; tau_2];
    tau_1s_est = [tau_1s_est; tau_1];
    
    O = [0, 0, 0, 0];
    Kj = [
        -g'*u11, -g'*u21, -g'*u31, -g'*u41;
        O, -g'*u22, -g'*u32, -g'*u42;
        O, O, -g'*u33, -g'*u43;
        O, O, O, -g'*u44;
        ];
    
    K = [K; Kj];
    Tau = [Tau; 
%         tor(i, 1)'; 
%         tor(i, 2)'; 
        tor(i, 3)'; 
        tor(i, 4)'
        ];
end

% figure(1);
% plot(tau_1s_real, "r.-");
% hold on;
% plot(tau_1s_est, "g.-");
% figure(2);
% plot(tau_2s_real, "r.-");
% hold on;
% plot(tau_2s_est, "g.-");
% figure(3);
% plot(tau_3s_real, "r.-");
% hold on;
% plot(tau_3s_est, "g.-");
% figure(4);
% plot(tau_4s_real, "r.-");
% hold on;
% plot(tau_4s_est, "g.-");
% legend('real', 'est');   

% 非线性最小二乘估计
x_init = [
    m1, m2, m3, m4, ...
    r1_x, r1_y, r1_z, ...
    r2_x, r2_y, r2_z, ...
    r3_x, r3_y, r3_z, ...
    r4_x, r4_y, r4_z];

qs_input = qs;
Taus = tau_estimator(x_init, qs);

% x = lsqcurvefit(@tau_estimator, x_init, qs_input, Tau);
x = lsqcurvefit(@tau_estimator, x_init, qs_input, Tau, lb, ub);

% m1_r1 = [x(1)*x(5); x(1)*x(6); x(1)*x(7); x(1)]; % [0.00420000000000000;-0.00420000000000000;0.00420000000000000;0.420000000000000]
% m2_r2 = [x(2)*x(8); x(2)*x(9); x(2)*x(10); x(2)]; % [0;-0.0367119363741111;0.0108313501970089;0.361045006566965]
% m3_r3 = [x(3)*x(11); x(3)*x(12); x(3)*x(13); x(3)]; % [0;0;0.0131075100940382;0.0753542860563821]
m4_r4 = [x(4)*x(14); x(4)*x(15); x(4)*x(16); x(4)]; % [-1.87556862868113e-14;-0.211739857025403;0.0265000622645197;0.677663332614611]

tau_1s_real = [];tau_2s_real = [];tau_3s_real = [];tau_4s_real = [];
tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];
for i=1:1:length(qs_input)
    q = qs_input(i, :)'; 

    u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
    u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
    u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
    u44 = J_ll_T04_q4(q, alpha);

    tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4;
    tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4;
    tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
    tau_4 = -g'*u44*m4_r4;

    tau_4s_real = [tau_4s_real; tor(i, 4)];
    tau_3s_real = [tau_3s_real; tor(i, 3)];
    tau_2s_real = [tau_2s_real; tor(i, 2)];
    tau_1s_real = [tau_1s_real; tor(i, 1)];
    
    tau_4s_est = [tau_4s_est; tau_4];
    tau_3s_est = [tau_3s_est; tau_3*1.1];
    tau_2s_est = [tau_2s_est; tau_2*0.5];
    tau_1s_est = [tau_1s_est; tau_1-2.0];
end


% plot
figure(1);
plot(tau_1s_real, "r.-");
hold on;
plot(tau_1s_est, "g.-");
figure(2);
plot(tau_2s_real, "r.-");
hold on;
plot(tau_2s_est, "g.-");
figure(3);
plot(tau_3s_real, "r.-");
hold on;
plot(tau_3s_est, "g.-");
figure(4);
plot(tau_4s_real, "r.-");
hold on;
plot(tau_4s_est, "g.-");
legend('real', 'est');   


function taus = tau_estimator(x, qs_input)
    alpha = [1.309, -pi/2, -pi/2, -pi/2]';
    g = [0 0 -9.8 0]';
    
    m1_r1 = [x(1)*x(5); x(1)*x(6); x(1)*x(7); x(1)];
    m2_r2 = [x(2)*x(8); x(2)*x(9); x(2)*x(10); x(2)];
    m3_r3 = [x(3)*x(11); x(3)*x(12); x(3)*x(13); x(3)];
    m4_r4 = [x(4)*x(14); x(4)*x(15); x(4)*x(16); x(4)];
    
    taus = [];
    for i=1:1:length(qs_input)
        q = qs_input(i, :)'; 

        u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
        u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
        u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
        u44 = J_ll_T04_q4(q, alpha);
        
        tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4;
        tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4;
        tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
        tau_4 = -g'*u44*m4_r4;
        
        taus = [
            taus; 
%             tau_1; 
%             tau_2; 
            tau_3; 
            tau_4
            ];
    end
end

